package SGBox

import (
	"fmt"
	"math"
	. "utils"
)

type World struct {
	Gravity Vec2
	Bodies  *Array
}

func NewWorld() *World {
	retWorld := &World{
		Bodies: new(Array),
	}
	fmt.Println("New World")
	return retWorld
}

func (world *World) AddBody(body *Body) {
	body.World = world
	world.Bodies.Add(body)
}

func (world *World) RemoveBody(body *Body) {
	world.Bodies.Remove(body)
}

func (world *World) Update(dt float64) {
	world.UpdateVelocity(dt)
	world.UpdateCollision(dt)
	world.UpdatePosition(dt)
}

func (world *World) UpdateVelocity(dt float64) {
	for i := 0; i < world.Bodies.Count(); i++ {
		curBody := world.Bodies.Get(i).(*Body)
		if curBody.Type == EnumBodyType_Kinematic || curBody.Type == EnumBodyType_Static {
			continue
		}
		stepVel := Vec2{world.Gravity.X * dt, world.Gravity.Y * dt}
		curBody.Velocity.X, curBody.Velocity.Y = curBody.Velocity.X+stepVel.X, curBody.Velocity.Y+stepVel.Y
	}
}

func (world *World) UpdatePosition(dt float64) {
	for i := 0; i < world.Bodies.Count(); i++ {
		curBody := world.Bodies.Get(i).(*Body)
		if curBody.Velocity.X == 0 && curBody.Velocity.Y == 0 {
			continue
		}

		stepVel := Vec2{curBody.Velocity.X * dt, curBody.Velocity.Y * dt}
		curBody.Pos.X = curBody.Pos.X + stepVel.X
		curBody.Pos.Y = curBody.Pos.Y + stepVel.Y
	}
}

func (world *World) UpdateCollision(dt float64) {
	for i := 0; i < world.Bodies.Count(); i++ {
		ebodyd := world.Bodies.Get(i).(*Body)
		ebodyd.Contacts.Clear()
		for j := i + 1; j < world.Bodies.Count(); j++ {
			ebodyall := world.Bodies.Get(j).(*Body)
			info := ebodyd.ContactInfo(ebodyall)
			if info != nil {

				ebodyd.Contacts.Add(ebodyall)
				ebodyall.Contacts.Add(ebodyd)

				ebodyd.TriggerEvent(ebodyall)
				ebodyall.TriggerEvent(ebodyd)

				if ebodyd.Type == EnumBodyType_Dynamic && ebodyall.Type != EnumBodyType_Kinematic {
					world.resolveCollision(ebodyd, ebodyall)
				}
				if ebodyall.Type == EnumBodyType_Dynamic && ebodyd.Type != EnumBodyType_Kinematic {
					world.resolveCollision(ebodyall, ebodyd)
				}
			}
		}
	}
}

func (world *World) resolveCollision(body1, body2 *Body) {
	dvec := Vec2{body1.Pos.X - body2.Pos.X, body1.Pos.Y - body2.Pos.Y}
	dvec.Normalize()
	absdx, absdy := math.Abs(dvec.X), math.Abs(dvec.Y)

	if math.Abs(absdx-absdy) == 0 {
		if dvec.X < 0 {
			//body1.Pos.X = body2.Pos.X
		} else {
			//body1.Pos.X = body2.Pos.X
		}

		if dvec.Y < 0 {
			//body1.Pos.Y = body2.MaxY()
		} else {
			//body1.Pos.Y = body2.MaxY() + body1.Shape.Height*0.5
		}

	} else if absdx > absdy {

		if dvec.X < 0 {

			if body1.Velocity.X > 0 {
				body1.Velocity.X = 0
			}
			body1.Pos.X = body2.MinX() - body1.Shape.Width*0.5
		} else {
			if body1.Velocity.X < 0 {
				body1.Velocity.X = 0
			}
			body1.Pos.X = body2.MaxX() + body1.Shape.Width*0.5
		}
	} else {

		if dvec.Y < 0 {
			if body1.Velocity.Y > 0 {
				body1.Velocity.Y = 0
			}

		} else {
			if body1.Velocity.Y < 0 {
				body1.Velocity.Y = 0
			}
			body1.Pos.Y = body2.MaxY() + body1.Shape.Height*0.5

		}

	}
}
